#ifndef ROBORTS_DECISION_WAIT_BEHAVIOR_H
#define ROBORTS_DECISION_WAIT_BEHAVIOR_H

#include "io/io.h"

#include "../blackboard/blackboard.h"
#include "../executor/chassis_executor.h"
#include "../executor/gimbal_executor.h"
#include "../behavior_tree/behavior_state.h"
#include "../behavior_tree/behavior_node.h"
#include "../proto/decision.pb.h"

#include "line_iterator.h"

namespace roborts_decision {
class WaitBehavior {
 public:
  WaitBehavior(ChassisExecutor* &chassis_executor, GimbalExecutor* &gimbal_executor, Blackboard* &blackboard) :
       blackboard_(blackboard),
       chassis_executor_(chassis_executor),
       gimbal_executor_(gimbal_executor) {
  
    twist_accel.twist.linear.x = 0;
    twist_accel.twist.linear.y = 0;
    twist_accel.twist.linear.z = 0;
    twist_accel.twist.angular.x = 0;
    twist_accel.twist.angular.y = 0;
    twist_accel.twist.angular.z = 0;
    gimbal_angle.yaw_mode = false;
    gimbal_angle.pitch_mode = false;
    gimbal_angle.yaw_angle = 0;
    gimbal_angle.pitch_angle = 0; 
  }

  void Run() {
auto executor_state = Update();
//if (executor_state != BehaviorState::CMD_RUNNING) {
    chassis_executor_->Execute(twist_accel);
    gimbal_executor_->Execute(gimbal_angle); }
 // }

  void Cancel() {
    chassis_executor_->Cancel();
    gimbal_executor_->Cancel();
  }

  BehaviorState Update() {
    return chassis_executor_->Update();
  }

  ~WaitBehavior() = default;

 private:
  //! executor
  ChassisExecutor* const chassis_executor_;
  GimbalExecutor* const gimbal_executor_;

  //! perception information
  Blackboard* const blackboard_;

  //! special twist_accel
  roborts_msgs::TwistAccel twist_accel;

  //! special gimbal_rate
  roborts_msgs::GimbalAngle gimbal_angle;
};
}


#endif //ROBORTS_DECISION_WAIT_BEHAVIOR_H
